/*
 *  CPE 474 lab 0 - modern graphics test bed
 *  draws a partial cube using a VBO and IBO 
 *  glut/OpenGL/GLSL application   
 *  Uses glm and local matrix stack
 *  to handle matrix transforms for a view matrix, projection matrix and
 *  model transform matrix
 *
 *  zwood 9/12 
 *  Copyright 2012 Cal Poly. All rights reserved.
 *
 *****************************************************************************/

#ifdef __APPLE__
#include "GLUT/glut.h"
#include <OPENGL/gl.h>
#endif

#ifdef __unix__
#include <GL/freeglut.h>
#endif

#ifdef _WIN32
#include <GL/glew.h>
#include <GL/glut.h>

#pragma comment(lib, "glew32.lib")
#pragma comment(lib, "OpenGL32.lib")
#pragma comment(lib, "glu32.lib")
#pragma comment(lib, "freeglut.lib")
#endif

#include <iostream>
#include <string>
#include <assert.h>
#include <stdio.h>

#include "glm/glm.hpp"
#include "glm/gtc/matrix_transform.hpp"
#include "glm/gtc/type_ptr.hpp"

#include "GLSL_helper.h"
#include "MStackHelp.h"

#include "GeometryCreator.h"

#include "prime.h"
#include "hash.h"
#include "kernel.h"
#include "Plane.h"
#include "Sphere.h"
#include "Box.h"


using namespace std;
using namespace glm;


////////////////
// Globals :( //
////////////////


particle_attributes part;
Node head;
Node *cur_draw = NULL;
HashTable *table;
Plane plane;
Sphere c_sphere;
Box c_box, c_box2;
int box_1 = 1;

// Parameters
unsigned int const StepSize = 10;
unsigned int WindowWidth = 1600, WindowHeight = 900;
static const float g_groundY = -3.0;
static const float g_groundSize = 50.0;
GLuint GrndBuffObj, GIndxBuffObj, GNormalBuffObj;
int g_GiboLen;

// Meshes
Mesh *sphere;
Mesh *r_sphere;
Mesh *r_box;
Mesh *r_box2;

// Variable Handles
GLuint aPosition;
GLuint aNormal;
GLuint uModelMatrix;
GLuint uNormalMatrix;
GLuint uViewMatrix;
GLuint uProjMatrix;
GLuint uColor;

// Shader Handle
GLuint ShadeProg;

//output file
FILE *out = NULL;
int printOut = 0;


// Program Variables
float Accumulator;
float CameraHeight;
float g_trans = 0.f;


RenderingHelper ModelTrans;

void SetProjectionMatrix()
{
    glm::mat4 Projection = glm::perspective(80.0f, ((float) WindowWidth)/ ((float)WindowHeight), 0.1f, 100.f);
    safe_glUniformMatrix4fv(uProjMatrix, glm::value_ptr(Projection));
}

void SetView()
{
    glm::mat4 View = glm::lookAt(vec3(0.f, g_trans/*CameraHeight*/, 5.f + CameraHeight), vec3(0.f, g_trans, 0.f), vec3(0.f, 1.f, 0.f));
    safe_glUniformMatrix4fv(uViewMatrix, glm::value_ptr(View));
}

void SetModel()
{
    safe_glUniformMatrix4fv(uModelMatrix, glm::value_ptr(ModelTrans.modelViewMatrix));
    safe_glUniformMatrix4fv(uNormalMatrix, glm::value_ptr(glm::transpose(glm::inverse(ModelTrans.modelViewMatrix))));
}

static void initGround() {
    float GrndPos[] = {
        -g_groundSize, g_groundY, -g_groundSize,
        -g_groundSize, g_groundY, g_groundSize,
        g_groundSize, g_groundY, -g_groundSize,
        g_groundSize, g_groundY, g_groundSize
    };

    float GrndNorm[] = {
        0.0, 1.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 1.0, 0.0
    };

    unsigned short idx[] = {0, 1, 2, 0, 2, 3};

    g_GiboLen = 6;                                                              
    glGenBuffers(1, &GrndBuffObj);                                              
    glBindBuffer(GL_ARRAY_BUFFER, GrndBuffObj);                                 
    glBufferData(GL_ARRAY_BUFFER, sizeof(GrndPos), GrndPos, GL_STATIC_DRAW);    

    glGenBuffers(1, &GNormalBuffObj);
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, GNormalBuffObj);
    glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(GrndNorm), GrndNorm, GL_STATIC_DRAW);

    glGenBuffers(1, &GIndxBuffObj);                                             
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, GIndxBuffObj);                        
    glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(idx), idx, GL_STATIC_DRAW);    
}

bool InstallShader(std::string const & vShaderName, std::string const & fShaderName)
{
    GLuint VS; // handles to shader object
    GLuint FS; // handles to frag shader object
    GLint vCompiled, fCompiled, linked; // status of shader

    VS = glCreateShader(GL_VERTEX_SHADER);
    FS = glCreateShader(GL_FRAGMENT_SHADER);

    // load the source
    char const * vSource = textFileRead(vShaderName);
    char const * fSource = textFileRead(fShaderName);
    glShaderSource(VS, 1, & vSource, NULL);
    glShaderSource(FS, 1, & fSource, NULL);

    // compile shader and print log
    glCompileShader(VS);
    printOpenGLError();
    glGetShaderiv(VS, GL_COMPILE_STATUS, & vCompiled);
    printShaderInfoLog(VS);

    // compile shader and print log
    glCompileShader(FS);
    printOpenGLError();
    glGetShaderiv(FS, GL_COMPILE_STATUS, & fCompiled);
    printShaderInfoLog(FS);

    if (! vCompiled || ! fCompiled)
    {
        std::cerr << "Error compiling either shader " << vShaderName << " or " << fShaderName << std::endl;
        return false;
    }

    // create a program object and attach the compiled shader
    ShadeProg = glCreateProgram();
    glAttachShader(ShadeProg, VS);
    glAttachShader(ShadeProg, FS);

    glLinkProgram(ShadeProg);

    // check shader status requires helper functions
    printOpenGLError();
    glGetProgramiv(ShadeProg, GL_LINK_STATUS, &linked);
    printProgramInfoLog(ShadeProg);

    glUseProgram(ShadeProg);

    // get handles to attribute data
    aPosition   = safe_glGetAttribLocation(ShadeProg, "aPosition");
    aNormal     = safe_glGetAttribLocation(ShadeProg, "aNormal");
    
    uColor          = safe_glGetUniformLocation(ShadeProg, "uColor");
    uProjMatrix     = safe_glGetUniformLocation(ShadeProg, "uProjMatrix");
    uViewMatrix     = safe_glGetUniformLocation(ShadeProg, "uViewMatrix");
    uModelMatrix    = safe_glGetUniformLocation(ShadeProg, "uModelMatrix");
    uNormalMatrix   = safe_glGetUniformLocation(ShadeProg, "uNormalMatrix");

    std::cout << "Sucessfully installed shader " << ShadeProg << std::endl;
    return true;
}

int in_volume(float x, float y, float z) {
    return 
     x >= -0.7937 && x <= 0.7937 && 
     y >= -0.7937 && y <= 0.7937 && 
     z >= -0.7937 && z <= 0.7937;
}

void rehash() {
    Node *temp;
    static int c = 0;

    temp = head.next;
    while (temp) {
        table->remove(temp->p);
        table->insert(temp->p);

        temp = temp->next;
    }

    //printf("Finished hash %d\n", c++);
}

void compute_density(Particle *p) {
    Node *list, *list_t;

    p->pos_prev = p->pos;
    p->density = p->pressure = 0;
    p->f_pressure = p->f_gravity = p->f_viscosity = 
     p->f_surface = glm::vec3(0.f);

    list = table->lookUpId(
     p->pos.x,
     p->pos.y,
     p->pos.z, 
     p->id);
    list_t = list;

    while (list_t) {
        /* calculate the density of each partcle */
        p->density += list_t->p->mass * 
         kernel(p->pos - list_t->p->pos, part.radius);
        list_t = list_t->next;
    }

    p->density += p->mass * kernel(p->pos - p->pos, part.radius);
    p->pressure = part.stiffness * 
     (p->density - part.density);

    delete list;
}

void calculate() {
    Node *list, *list_t, *temp;
    Particle *p;

    rehash();
    /* compute all the densities and pressures */
    temp = head.next;
    while (temp) {
        compute_density(temp->p);
        temp = temp->next;
    }

    temp = head.next;
    while (temp) {
        p = temp->p;
        list = table->lookUpId(
         p->pos.x,
         p->pos.y,
         p->pos.z,
         p->id);
        list_t = list;

        p->normal *= 0.f;
        p->f_pressure *= 0.f;
        p->f_surface *= 0.f;
        p->f_viscosity *= 0.f;
        while (list_t) {
            p->f_pressure += (
             p->pressure / (p->density * p->density) +
             list_t->p->pressure / (list_t->p->density * list_t->p->density)) *
             list_t->p->mass * 
             grad_pressure(p->pos - list_t->p->pos, part.radius);

            p->f_viscosity += (list_t->p->v - p->v) * list_t->p->mass *
             laplacian_viscosity(p->pos - list_t->p->pos, part.radius);

            p->normal += list_t->p->mass / list_t->p->density *
             grad_kernel(p->pos - list_t->p->pos, part.radius);

            p->f_surface += list_t->p->mass / list_t->p->density *
             laplacian_kernel(p->pos - list_t->p->pos, part.radius);

            list_t = list_t->next;
        }
        delete list;

        p->f_gravity = p->density * glm::vec3(0.f, -9.81f, 0.f);
        
        if (glm::length(p->normal) < part.threshold)
            p->f_surface *= 0.f;
        else 
            p->f_surface *= -part.surface_tension * glm::normalize(p->normal);

        p->f_pressure *= -0.1 * p->density;
        p->f_viscosity *= part.viscosity / p->density;

        temp = temp->next;
    }
}

void print_part(Particle *p) {
    printf(
     "\tID: %d\n\tdensity: %f\n\tpressure: %f\n\tvelocity: <%f, %f, %f>"
     "\n\taccel: <%f, %f, %f>\n\tf_viscosity: <%f, %f, %f>\n\t"
     "f_pressure: <%f, %f, %f>\n\tf_surface: <%f, %f, %f>\n\t"
     "f_gravity: <%f, %f, %f>\n\t"
     "pos: <%f, %f, %f>\n",
     p->id, p->density, p->pressure, p->v.x, p->v.y, p->v.z,
     p->a.x, p->a.y, p->a.z, 
     p->f_viscosity.x, p->f_viscosity.y, p->f_viscosity.z,
     p->f_pressure.x, p->f_pressure.y, p->f_pressure.z,
     p->f_surface.x, p->f_surface.y, p->f_surface.z, 
     p->f_gravity.x, p->f_gravity.y, p->f_gravity.z,
     p->pos.x, p->pos.y, p->pos.z);
}

void check_extreme(glm::vec3 val, Particle *p1, Particle *p2, vec3 old, 
 float chk) {
    vec3 temp = old - val;
    if (abs(temp.x) > chk ||
        abs(temp.y) > chk ||
        abs(temp.z) > chk) {
        printf("interaction between %d and %d exedes expected values\n", 
         p1->id, p2->id);

        printf("New Val: <%f, %f, %f>\n"
        "Old Val: <%f, %f, %f>\n""This particle: \n", 
        val.x, val.y, val.z,
        old.x, old.y, old.z);
        print_part(p1);
        printf("Other particle: \n");
        print_part(p2);

        exit(1);
    }
}

void compute_internal(Particle *p) {
    Node *list, *list_t;
    glm::vec3 temp;

    list = table->lookUpId(
     p->pos.x,
     p->pos.y,
     p->pos.z,
     p->id);
    list_t = list;

    p->f_pressure *= 0.f;
    p->f_viscosity *= 0.f;
    while (list_t) {
        temp = p->f_pressure;
        p->f_pressure +=
         (p->pressure / (p->density * p->density) +
         list_t->p->pressure / (list_t->p->density * list_t->p->density)) *
         list_t->p->mass *
         grad_pressure(p->pos - list_t->p->pos, part.radius);
        
        p->f_viscosity +=
         (list_t->p->v - p->v) * list_t->p->mass * 
         laplacian_viscosity(p->pos - list_t->p->pos, part.radius);

        //check_extreme(p->f_pressure, p, list_t->p, temp, 50.f);

        list_t = list_t->next;
    }

    //p->f_pressure *= -p->density;
    p->f_pressure *= 0.1f * -p->density;
    p->f_viscosity *= part.viscosity / p->density;

    delete list;
}

void compute_external(Particle *p) {
    Node *list, *list_t;

    list = table->lookUpId(
     p->pos.x,
     p->pos.y,
     p->pos.z,
     p->id);
    list_t = list;

    p->normal = glm::vec3(0.f);
    while (list_t) {
        p->normal += list_t->p->mass / list_t->p->density *
         grad_kernel(p->pos - list_t->p->pos, part.radius);
        
        list_t = list_t->next;
    }

    delete list;

    /* computer surface tension and gravity on each particle */
    p->f_gravity = p->density * glm::vec3(0.f, -9.81f, 0.f);

    if (glm::length(p->normal) < part.threshold) {
        p->f_surface = glm::vec3(0.f);

        return;
    }

    list = table->lookUpId(
     p->pos.x,
     p->pos.y,
     p->pos.z,
     p->id);
    list_t = list;


    p->f_surface *= 0.f;
    while (list_t) {
        p->f_surface +=
         list_t->p->mass / list_t->p->density *
         laplacian_kernel(p->pos - list_t->p->pos, part.radius);

        list_t = list_t->next;
    }

    delete list;
    p->f_surface *= -part.surface_tension * 
     glm::normalize(p->normal);
}

void initial_calc() {
    Node *temp;
    float avg_dens = 0.0f;

    rehash();
    temp = head.next;
    while (temp) {
        compute_density(temp->p);
        temp = temp->next;
    }

    /* compute initial internal forces on each particle */
    temp = head.next;
    while (temp) {
        compute_internal(temp->p);
     //   avg_dens += temp->p->density;
        temp = temp->next;
    }

    //avg_dens /= part.count;
    //part.threshold = sqrt(avg_dens / part.kernel);

    /* compute external forces for each particle */
    temp = head.next;
    while (temp) {
        compute_external(temp->p);
        //temp->p->color = temp->p->f_viscosity;
        temp = temp->next;
    }
}

void check_viscosity(Particle *p) {
    float dot, length;

    if (glm::length(part.time_step * p->f_viscosity / p->density) > 100.0) {
        printf("Large viscosity\n");
        printf("%d\n\tViscosity <%f, %f, %f>\n",
         p->id,
         p->f_viscosity.x,
         p->f_viscosity.y,
         p->f_viscosity.z);
        exit(1);
    }
}

void compute_step(Particle *p) {
    float depth;
    vec3 temp, temp2;
    //check_viscosity(p);

    //p->f_gravity = vec3(0.f);
    //p->f_viscosity *= 0.f;
    //p->f_surface *= 0.f;
    p->a = (p->f_gravity + p->f_viscosity +
     p->f_pressure + p->f_surface) / p->density;
    
    glm::vec3 v_half_step = p->v_half + part.time_step * 
     p->a;
    
    p->pos += part.time_step * v_half_step;
    /*if (c_sphere.check_collide(p->pos_prev, p->pos)) {
        temp = c_sphere.collide(p->pos_prev, p->pos);
        depth = glm::length(p->pos - temp);
        p->pos = temp;
        v_half_step += -(1.f + part.restitution * depth / 
         ( part.time_step * glm::length(p->v))) * 
         glm::dot(p->v, c_sphere.getNormal(temp)) * c_sphere.getNormal(temp);
    }
    else*/ 
    temp = p->pos_prev;
    if (box_1 && c_box.check_collide(temp, p->pos)) {
        temp = c_box.collide(temp, p->pos);
        depth = glm::length(temp - p->pos);
        temp2 = c_box.getNormal(/*p->pos_prev*/temp);
        v_half_step += -(1.f + part.restitution * depth /
         (part.time_step * glm::length(v_half_step))) *
         glm::dot(v_half_step, temp2) * temp2;
        p->pos = temp /*+ v_half_step * part.time_step*/;
    }
    else if (c_box2.check_collide(temp, p->pos)) {
        temp = c_box2.collide(temp, p->pos);
        depth = glm::length(temp - p->pos);
        temp2 = c_box2.getNormal(/*p->pos_prev*/temp);
        v_half_step += -(1.f + part.restitution * depth /
         (part.time_step * glm::length(v_half_step))) *
         glm::dot(v_half_step, temp2) * temp2;
        p->pos = temp /*+ v_half_step * part.time_step*/;
    }
    
    /*else if (0 && plane.check_collide(temp, p->pos)) {
        temp = plane.collide(temp, p->pos);
        depth = glm::length(p->pos - temp);
        p->pos = temp/* + plane.normal * part.time_step/;
        v_half_step += -(1.f + part.restitution * depth / 
         (part.time_step * glm::length(v_half_step))) * 
         glm::dot(v_half_step, plane.normal) * plane.normal;
        //p->v_half += -(1.f + part.restitution /* depth / 
        // (part.time_step * glm::length(p->v))/) * 
        // glm::dot(p->v, plane.normal) * plane.normal;
        //p->v = p->v_half = v_half_step;
        /*temp = p->pos;
        p->pos += part.time_step * v_half_step;*/
    //}

    p->v = (p->v_half + v_half_step) / 2.0f;
    p->v_half = v_half_step;
}

void compute() {
    Node *temp, *list, *list_t;

    /* Computer initial density and initial pressure of each particle */
    temp = head.next;
    while (temp) {
        compute_density(temp->p);
        temp = temp->next;
    }

    /* compute initial internal forces on each particle */
    temp = head.next;
    while (temp) {
        compute_internal(temp->p);
        temp = temp->next;
    }

    /* compute normal for each particle */
    temp = head.next;
    while (temp) {
        compute_external(temp->p);
        temp = temp->next;
    }

    temp = head.next;
    while (temp) {
        compute_step(temp->p);
        temp = temp->next;
    }

    rehash();
}

void create_particle(float x, float y, float z, int id) {
    Node *temp;
    
    temp = head.next;

    head.next = (Node *) calloc(sizeof(Node), 1);
    if (!head.next) {
        perror("malloc");
        exit(EXIT_FAILURE);
    }

    head.next->next = temp;
    head.next->p = new Particle();
    head.next->p->pos = vec3(x, y, z);
    head.next->p->color = vec3(.1f, 0.9f, 0.78f);
    head.next->p->mass = part.mass;
    head.next->p->id = id;
    head.next->p->v_half = head.next->p->v = vec3(0.f);
}

float jitter(float cell) {
    int r = rand();
    float ret;

    r = (r % 1000) - 500;

    ret = ((float) r) / 500.f;
    ret *= cell / 2.f;

    return ret;
}

void create_particles() {
    float x, y, z;
    vec3 st, en;
    int count;
    float h;
    Node *temp, *list, *list_t;

    st = vec3(-.400f, -.400f, -.400f);
    en = vec3(.400f, .400f, .400f);
    count = 0;

    h = pow(3.f * (0.800 * 0.800 * 0.800) *
     part.kernel / 4.f / PI / 1000.f, 1.f / 3.f) / 1.6;
    //h = 0.0457 * pow(part.kernel, 1.f / 3.f);
    for (x = st.x /*+ 0.25 * h*/; x < en.x; x += h) {
        for (y = st.y /*+ 0.25 * h*/; y < en.y; y += h) {
            for (z = st.z /*+ 0.25 * h*/; z < en.z; z += h) {
                if (in_volume(x, y, z)) {
                    create_particle(
                     x + jitter(h), 
                     y + jitter(h), 
                     z + jitter(h), 
                     ++count);
                }
            }
        }
    }

    part.radius = 
     pow(3.f * 0.5f * part.kernel / 4.f / 3.1415926 / count, 1.f / 3.f);
    table = new HashTable(count, part.radius);
    temp = head.next;
    while (temp) {
        temp->p->mass = 0.5f * part.density / count;
        temp->p->density = part.density;
        table->insert(temp->p);
        temp = temp->next;
    }

    part.count = count;
    //part.radius = 0.0457;
    part.render = pow(1.f * 1.0f / count, 1.f / 3.f);
    //part.density /= count;
}

void Initialize()
{
    glClearColor(0.8f, 0.8f, 1.0f, 1.0f);

    glClearDepth(1.0f);
    glDepthFunc(GL_LEQUAL);
    glEnable(GL_DEPTH_TEST);

    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);

    ModelTrans.useModelViewMatrix();
    ModelTrans.loadIdentity();

    head.next = NULL;
    create_particles();
}

void Draw() {
    Node *temp;
    float r_scale;

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glUseProgram(ShadeProg);

    SetProjectionMatrix();
    SetView();
    
    ModelTrans.loadIdentity();

    temp = head.next;
    while (temp) {
        ModelTrans.pushMatrix();

            ModelTrans.rotate(Accumulator * 35.f, vec3(0, 1, 0));
            ModelTrans.translate(temp->p->pos);
            
            r_scale = 0.89 * pow(3.f * temp->p->mass / 4.f / PI / 
             temp->p->density, 1.f/3.f);
            ModelTrans.scale(
             r_scale,
             r_scale,
             r_scale);
            SetModel();

            safe_glEnableVertexAttribArray(aPosition);
            glBindBuffer(GL_ARRAY_BUFFER, sphere->PositionHandle);
            safe_glVertexAttribPointer(aPosition, 3, GL_FLOAT, GL_FALSE, 0, 0);

            safe_glEnableVertexAttribArray(aNormal);
            glBindBuffer(GL_ARRAY_BUFFER, sphere->NormalHandle);
            safe_glVertexAttribPointer(aNormal, 3, GL_FLOAT, GL_FALSE, 0, 0);

            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, sphere->IndexHandle);

            glUniform3f(uColor, temp->p->color.x, temp->p->color.y, 
             temp->p->color.z);

            glDrawElements(GL_TRIANGLES, sphere->IndexBufferLength,
             GL_UNSIGNED_SHORT, 0);

            safe_glDisableVertexAttribArray(aPosition);
            safe_glDisableVertexAttribArray(aNormal);

        ModelTrans.popMatrix();

        temp = temp->next;
    }


    ModelTrans.pushMatrix();
        ModelTrans.translate(glm::vec3(10.f, -part.render, 10.f));
        SetModel();
        safe_glEnableVertexAttribArray(aPosition);
        
        glBindBuffer(GL_ARRAY_BUFFER, GrndBuffObj);
        safe_glVertexAttribPointer(aPosition, 3, GL_FLOAT, GL_FALSE, 0, 0);

        safe_glEnableVertexAttribArray(aNormal);
        glBindBuffer(GL_ARRAY_BUFFER, GNormalBuffObj);
        
        glUniform3f(uColor, 0.f, 0.8f, 0.1f);

        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, GIndxBuffObj);
        glDrawElements(GL_TRIANGLES, g_GiboLen, GL_UNSIGNED_SHORT, 0);

        safe_glDisableVertexAttribArray(aPosition);
        safe_glDisableVertexAttribArray(aNormal);
    ModelTrans.popMatrix();

    ModelTrans.pushMatrix();
        ModelTrans.rotate(Accumulator * 35.f, vec3(0, 1, 0));
        SetModel();
        safe_glEnableVertexAttribArray(aPosition);
        glBindBuffer(GL_ARRAY_BUFFER, r_box->PositionHandle);
        safe_glVertexAttribPointer(aPosition, 3, GL_FLOAT, GL_FALSE, 0, 0);

        safe_glEnableVertexAttribArray(aNormal);
        glBindBuffer(GL_ARRAY_BUFFER, r_box->NormalHandle);
        safe_glVertexAttribPointer(aNormal, 3, GL_FLOAT, GL_FALSE, 0, 0);

        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, r_box->IndexHandle);

        glUniform3f(uColor, .5f, .0f, 0.5f);

        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
        glDisable(GL_CULL_FACE);
        glDrawElements(GL_TRIANGLES, r_box->IndexBufferLength,
         GL_UNSIGNED_SHORT, 0);

        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        glEnable(GL_CULL_FACE);
        safe_glDisableVertexAttribArray(aPosition);
        safe_glDisableVertexAttribArray(aNormal);
    ModelTrans.popMatrix();

    ModelTrans.pushMatrix();
        ModelTrans.rotate(Accumulator * 35.f, vec3(0, 1, 0));
        ModelTrans.translate(vec3(0.5f, 0.0f, 0.0f));
        SetModel();
        safe_glEnableVertexAttribArray(aPosition);
        glBindBuffer(GL_ARRAY_BUFFER, r_box2->PositionHandle);
        safe_glVertexAttribPointer(aPosition, 3, GL_FLOAT, GL_FALSE, 0, 0);

        safe_glEnableVertexAttribArray(aNormal);
        glBindBuffer(GL_ARRAY_BUFFER, r_box2->NormalHandle);
        safe_glVertexAttribPointer(aNormal, 3, GL_FLOAT, GL_FALSE, 0, 0);

        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, r_box2->IndexHandle);

        glUniform3f(uColor, .5f, .0f, 0.5f);

        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
        glDisable(GL_CULL_FACE);
        glDrawElements(GL_TRIANGLES, r_box2->IndexBufferLength,
         GL_UNSIGNED_SHORT, 0);

        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        glEnable(GL_CULL_FACE);
        safe_glDisableVertexAttribArray(aPosition);
        safe_glDisableVertexAttribArray(aNormal);
    ModelTrans.popMatrix();


    glUseProgram(0);
    glutSwapBuffers();
    glutPostRedisplay();
    printOpenGLError();
}

void Reshape(int width, int height)
{
    WindowWidth = width;
    WindowHeight = height;
    glViewport(0, 0, width, height);
}

void print_all(Node *n) {
    Node *temp;
    static int print_num = 0;

    fprintf(stderr,
        "=============================================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "=====================PRINT %d=================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "=============================================================\n"
        "part.radius: %f\n"
        "part.density: %f\n",
        ++print_num, part.radius, part.density);

    temp = n;
    while (temp) {
        fprintf(stderr, "%d:\n\tpos: <%f, %f, %f>"
         "\n\tpressure: <%f, %f, %f>\n\tviscosity: <%f, %f, %f>"
         "\n\tgravity <%f, %f, %f>\n\tsurface tension: <%f, %f, %f>\n\t"
         "accel: <%f, %f, %f>\n\tvelocity: <%f, %f, %f>\n\t"
         "v_half: <%f, %f, %f>\n\t"
         "normal: <%f, %f, %f>\n\t"
         "pressure (particle): %f\n\t"
         "density: %f\n\tmass; %f\n",
         temp->p->id, 
         temp->p->pos.x, temp->p->pos.y, temp->p->pos.z,
         temp->p->f_pressure.x,temp->p->f_pressure.y,temp->p->f_pressure.z, 
         temp->p->f_viscosity.x,temp->p->f_viscosity.y,temp->p->f_viscosity.z, 
         temp->p->f_gravity.x,temp->p->f_gravity.y,temp->p->f_gravity.z,
         temp->p->f_surface.x,temp->p->f_surface.y,temp->p->f_surface.z,
         temp->p->a.x, temp->p->a.y, temp->p->a.z,
         temp->p->v.x, temp->p->v.y, temp->p->v.z,
         temp->p->v_half.x, temp->p->v_half.y, temp->p->v_half.z,
         temp->p->normal.x, temp->p->normal.y, temp->p->normal.z,
         temp->p->pressure, temp->p->density, temp->p->mass);

        temp = temp->next;
    }

}

bool among(Node *list, Particle *p) {
    Node *temp;

    temp = list;
    while (temp) {
        if (temp->p->id == p->id)
            return true;

        temp = temp->next;
    }

    return false;
}

void sanity_test(Node *check, Particle *p) {
    Node *temp;

    temp = head.next;

    while (temp) {
        if (length(temp->p->pos - p->pos_prev) <= part.radius && 
         p->id != temp->p->id &&
         !among(check, temp->p)) {
            printf("SANITY FAILED\n");
            printf("prev_pos: <%f, %f, %f>\n"
             "comp_pos: <%f, %f, %f>\n"
             "length: %f\n",
             p->pos_prev.x,
             p->pos_prev.y,
             p->pos_prev.z,
             temp->p->pos.x,
             temp->p->pos.y,
             temp->p->pos.z,
             length(p->pos_prev - temp->p->pos));
        }
        temp = temp->next;
    }
}

void color_radius(Particle *p, vec3 color) {
    Node *list, *temp;

    list = table->lookUpId(p->pos_prev.x, p->pos_prev.y, p->pos_prev.z, p->id);
    //print_list(list);

    temp = list;
    while (temp) {
        temp->p->color = color;
        temp = temp->next;
    }

    free(list);
}

void print_header() {
    fprintf(out, "0 %d %f\n", head.next->p->id, part.radius);
}

void print_frame(int frame) {
    fprintf(out, "f %d\n", frame);
    printf("Frame %d\n", frame);
}

void print_info(Particle *p, int num) {
    fprintf(out, "p %d\n %f %f %f\n", num, 
     p->pos.x, 
     p->pos.y, 
     p->pos.z); 
}

void Keyboard(unsigned char key, int x, int y)
{
    static int calc = 1;
    int i = 0;
    static int frame = 0;
    switch (key)
    {
    
    // Camera up/down
    case 'w':
        CameraHeight -= 0.1f;
        break;
    case 's':
        CameraHeight += 0.1f;
        break;
    case 'a':
        Accumulator -= StepSize * 0.01f;
        break;
    case 'd':
        Accumulator += StepSize * 0.01f;
        break;

    case 'r':
        g_trans += StepSize * 0.01f;
        break;
    case 'f':
        g_trans -= StepSize * 0.01f;
        break;

    case 'p':
        print_all(head.next);
        break;


    case '.':
    case '>':
        if (printOut)
            break;
        if (cur_draw == NULL || cur_draw->next == NULL) {
            if (cur_draw) {
                cur_draw->p->color = vec3(0.1f, 0.9f, 0.78f);
                compute_step(cur_draw->p);
                printf("%d:\n\tpos: <%f, %f, %f>"
                 "\n\tpressure: <%f, %f, %f>\n\tviscosity: <%f, %f, %f>"
                 "\n\tgravity <%f, %f, %f>\n\tsurface tension: <%f, %f, %f>\n\t"
                 "accel: <%f, %f, %f>\n\tvelocity: <%f, %f, %f>\n\t"
                 "v_half: <%f, %f, %f>\n\t"
                 "normal: <%f, %f, %f>\n\t"
                 "pressure (particle): %f\n\t"
                 "density: %f\n",
                 cur_draw->p->id, 
                 cur_draw->p->pos.x, cur_draw->p->pos.y, cur_draw->p->pos.z,
                 cur_draw->p->f_pressure.x,cur_draw->p->f_pressure.y,cur_draw->p->f_pressure.z, 
                 cur_draw->p->f_viscosity.x,cur_draw->p->f_viscosity.y,cur_draw->p->f_viscosity.z, 
                 cur_draw->p->f_gravity.x,cur_draw->p->f_gravity.y,cur_draw->p->f_gravity.z,
                 cur_draw->p->f_surface.x,cur_draw->p->f_surface.y,cur_draw->p->f_surface.z,
                 cur_draw->p->a.x, cur_draw->p->a.y, cur_draw->p->a.z,
                 cur_draw->p->v.x, cur_draw->p->v.y, cur_draw->p->v.z,
                 cur_draw->p->v_half.x, cur_draw->p->v_half.y, cur_draw->p->v_half.z,
                 cur_draw->p->normal.x, cur_draw->p->normal.y, cur_draw->p->normal.z,
                 cur_draw->p->pressure, cur_draw->p->density);
                cur_draw = head.next;
                initial_calc();
                break;
            }
            cur_draw = head.next;
            initial_calc();
        }
        else {
            cur_draw->p->color =  vec3(.1f, 0.9f, 0.78f);
            color_radius(cur_draw->p, vec3(.1f, 0.9f, 0.78f));
            cur_draw = cur_draw->next;
        }

        compute_step(cur_draw->p);
        cur_draw->p->color = glm::vec3(1.0f, 0.0f, 0.0f);
        color_radius(cur_draw->p, vec3(0.0f, 1.f, 0.f));
        printf("%d:\n\tpos: <%f, %f, %f>"
         "\n\tpressure: <%f, %f, %f>\n\tviscosity: <%f, %f, %f>"
         "\n\tgravity <%f, %f, %f>\n\tsurface tension: <%f, %f, %f>\n\t"
         "accel: <%f, %f, %f>\n\tvelocity: <%f, %f, %f>\n\t"
         "v_half: <%f, %f, %f>\n\t"
         "normal: <%f, %f, %f>\n\t"
         "pressure (particle): %f\n\t"
         "density: %f\n",
         cur_draw->p->id, 
         cur_draw->p->pos.x, cur_draw->p->pos.y, cur_draw->p->pos.z,
         cur_draw->p->f_pressure.x,cur_draw->p->f_pressure.y,cur_draw->p->f_pressure.z, 
         cur_draw->p->f_viscosity.x,cur_draw->p->f_viscosity.y,cur_draw->p->f_viscosity.z, 
         cur_draw->p->f_gravity.x,cur_draw->p->f_gravity.y,cur_draw->p->f_gravity.z,
         cur_draw->p->f_surface.x,cur_draw->p->f_surface.y,cur_draw->p->f_surface.z,
         cur_draw->p->a.x, cur_draw->p->a.y, cur_draw->p->a.z,
         cur_draw->p->v.x, cur_draw->p->v.y, cur_draw->p->v.z,
         cur_draw->p->v_half.x, cur_draw->p->v_half.y, cur_draw->p->v_half.z,
         cur_draw->p->normal.x, cur_draw->p->normal.y, cur_draw->p->normal.z,
         cur_draw->p->pressure, cur_draw->p->density);
        break;

    case 'c':
        if (cur_draw != NULL) {
            cur_draw->p->color = glm::vec3(0.f, 0.78f, 0.9f);
            cur_draw = cur_draw->next;
            while (cur_draw) {
                compute_step(cur_draw->p);
                cur_draw->p->color = glm::vec3(0.f, 0.78f, 0.9f);
                if (cur_draw->p->pressure > 0) {
                    cur_draw->p->color = glm::vec3(0.f, 0.00f, 
                     cur_draw->p->pressure / 1000.0f);
                }
                else {
                    cur_draw->p->color = glm::vec3(
                     -cur_draw->p->pressure / 1000.0f, 0.f, 0.f);
                }
                cur_draw = cur_draw->next;
            }
        }
        else {
            i = 0;
            //initial_calc();
            calculate();
            cur_draw = head.next;
            if (printOut && 
             (frame % ((int) ((1.0/part.time_step) / 24.0))) == 0) {
                print_frame(frame / ((int) ((1.0/part.time_step) / 24.0)));
                frame++;
            }
            else if (printOut) {
                frame++;
            }
            while (cur_draw) {
                compute_step(cur_draw->p);
                if (printOut &&
                 ((frame - 1) % ((int) ((1.0/part.time_step) / 24.0))) == 0) {
                    print_info(cur_draw->p, i);
                    i++;
                }
                cur_draw->p->color = glm::vec3(0.f, 0.78f, 0.9f);
                if (cur_draw->p->pressure > 0) {
                    cur_draw->p->color = glm::vec3(0.f, 0.00f, 
                     cur_draw->p->pressure / 100.0f);
                }
                else {
                    cur_draw->p->color = glm::vec3(
                     -cur_draw->p->pressure / 100.0f, 0.f, 0.f);
                }

                cur_draw = cur_draw->next;
            }
        }
        printf("Finished calculating %d\n", calc++);
        break;

    // Toggle wireframe
    case 'n':
        //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
        //glDisable(GL_CULL_FACE);
        box_1 = 0;
        break;
    case 'm':
        /* toggles printing of particle information */
        if (out != NULL && !printOut) {
            printOut = 1;
            print_header();
        }
        break;

    // Quit program
    case 'q': case 'Q' :
        
#ifdef __unix__
        glutLeaveMainLoop();
#endif
        break;
    
    }
}

void Timer(int param)
{
    glutTimerFunc(StepSize, Timer, 1);
}

int main(int argc, char *argv[])
{
    if (argc == 2) {
        out = fopen(argv[1], "w");
        if (!out)
            perror("fopen");
    }
    // Initialize Global Variables
    Accumulator = 0.f;
    CameraHeight = 0.f;

    // Glut Setup
    glutInit(& argc, argv);
    glutInitWindowPosition(100, 100);
    glutInitWindowSize(WindowWidth, WindowHeight);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
    glutCreateWindow("Geometry Creator Demo");
    glutDisplayFunc(Draw);
    glutReshapeFunc(Reshape);
    glutKeyboardFunc(Keyboard);
    glutTimerFunc(StepSize, Timer, 1);

    // GLEW Setup (Windows only)
#ifdef _WIN32
    GLenum err = glewInit();
    if (GLEW_OK != err)
    {
        std::cerr << "Error initializing glew! " << glewGetErrorString(err) << std::endl;
        return 1;
    }
#endif

    // OpenGL Setup
    Initialize();
    getGLversion();

    // Shader Setup
    if (! InstallShader("Diffuse.vert", "Diffuse.frag"))
    {
        printf("Error installing shader!\n");
        return 1;
    }
    
    initGround();
    sphere = GeometryCreator::CreateSphere(glm::vec3(1.f), 6, 3);
    plane.normal = glm::vec3(0.f, 1.f, 0.f);
    plane.distance = 1.f;
    c_sphere.center = glm::vec3(0.f, -2.5f, 0.f);
    c_sphere.radius = 0.4;
    c_box.min = glm::vec3(-0.5f, -.5f, -.5f);
    c_box.max = glm::vec3(.5f, .5f, .5f);
    c_box2.min = glm::vec3(-0.5f, -0.5f, -0.5f);
    c_box2.max = glm::vec3(1.5f, 0.5f, 0.5f);
    r_sphere = GeometryCreator::CreateSphere(glm::vec3(0.4f), 9, 5);
    r_box = GeometryCreator::CreateCube(glm::vec3(1.f));
    r_box2 = GeometryCreator::CreateCube(glm::vec3(2.f, 1.f, 1.f));
    glutMainLoop();

    if (out)
        fclose(out);

    delete head.next;
    delete table;
    return 0;
}
